#include "fintrf.h"
!     
!=======================================================================
! Gateway subroutine
subroutine mexfunction(nlhs, plhs, nrhs, prhs)

! Declarations
implicit none

! mexFunction arguments:
mwPointer plhs(*), prhs(*)
integer nlhs, nrhs

! Function declarations:
mwPointer mxGetPr
mwPointer mxCreateDoubleMatrix, mxCreateNumericArray
mwPointer mxGetM, mxGetN

! Pointers to input/output mxArrays:
mwPointer pr1 ,pr2 ,pr3 ,&
          pr1_out

! Array information:
mwPointer n1,n2

! Arguments for mxCreateNumericArray
integer*4 mxClassIDFromClassName
integer*4 classid
integer*4 complexflag
mwSize ndim3,ndim4,ndim5
mwSize dims3(3),dims4(4),dims5(5)

!-----------------------------------------------------------------------
! Check for proper number of arguments. 
if(nrhs .ne. 3) then
   call mexErrMsgIdAndTxt ('MATLAB:dblmat:nInput','3 input required.')
end if
if(nlhs .ne. 1) then
   call mexErrMsgIdAndTxt ('MATLAB:dblmat:nOutput','1 output required.')
end if
         
! Create Fortran arrays from the input argument.
pr1  = mxGetPr(prhs(1))
pr2  = mxGetPr(prhs(2))
pr3  = mxGetPr(prhs(3))

! Get the size of the input array.
call mxCopyPtrToInteger4(pr2, n1, 1)
call mxCopyPtrToInteger4(pr3, n2, 1)

! Create matrix for the return argument.
plhs(1) = mxCreateDoubleMatrix(n1,n1, 0)
pr1_out = mxGetPr(plhs(1))


! Call the computational routine.
call fortranSub(%VAL(pr1_out),&
   %VAL(pr1) ,%VAL(pr2) ,%VAL(pr3))

return
end

!===============================================================
!     S=triag(A) uses a Householder transformation of the rectangular
!     matrix A to produce a square and upper triangular matrix S with
!     the property S*S' = A*A'.
!     Literature:
!     The algorithm is described in Grewal & Andrews: "Kalman Filtering - 
!     Theory and Practice" Prentice-Hall, 1993, pp. 234. However, there 
!     are certain cases in which their implementation fails. These have been
!     handled as described in G.W. Stewart: "Matrix Algorithms, 
!     Vol. 1: Basic Decompositions", SIAM 1988.
!
! Original Written by: Magnus Norgaard, IMM/IAU, Technical University of Denmark in Matlab
! Implemented in Fortran by Martin Møller Andreasen, 

subroutine fortranSub(S,A,n1,n2) 

IMPLICIT NONE
INTEGER,INTENT(IN)  :: n1,n2
REAL*8, INTENT(IN)  :: A(n1,n2)
REAL*8, INTENT(OUT) :: S(n1,n1)
INTEGER n,rn,r,k,i
REAL*8, ALLOCATABLE :: v(:,:),u(:,:),A_temp(:,:)
REAL*8  ny,sum_temp
n  = n1          ! Rows and columns of A
rn = n2
r = rn-n

ALLOCATE(v(n,1),u(1,rn),A_temp(n,rn))
A_temp = A
v = 0._8
u = 0._8
S = 0._8

DO k = n,1,-1
   u(1,1:r+k) = A_temp(k,1:r+k)

   ! The norm of u(1:r+k)
   sum_temp = 0._8
   DO i=1,r+k
      sum_temp = sum_temp + u(1,i)**2
   END DO
   ny = SQRT(sum_temp) !norm(u(1,1:r+k));
   IF (ny == 0._8) THEN
     u(1,r+k) = SQRT(2._8)
   ELSE
      u(1,1:r+k) = u(1,1:r+k)/ny
      IF (u(1,r+k)>=0) THEN        
        u(1,r+k) = u(1,r+k) + 1._8
        ny = -ny
      ELSE
        u(1,r+k) = u(1,r+k) - 1._8
      END IF
      u(1,1:r+k) = u(1,1:r+k)/SQRT(ABS(u(1,r+k)))
   END IF
   v(1:k-1,1:1) = MATMUL(A_temp(1:k-1,1:r+k),TRANSPOSE(u(1:1,1:r+k)))
   A_temp(1:k-1,1:r+k) = A_temp(1:k-1,1:r+k) - MATMUL(v(1:k-1,1:1),u(1:1,1:r+k))
   S(1:k-1,k) = A_temp(1:k-1,r+k)
   S(k,k)=ny
END DO

DEALLOCATE(v,u,A_temp)

END SUBROUTINE fortranSub

